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Tele- Autonomous Control 
Involving Contacts 

- The Applications of A High Precision Laser Line Range Sensor - 

1 Introduction 

The difficulty in applying tele-autonomous control to do assembly or other similar tasks 
is the existence of uncertainties of the environment including the telerobot itself. To deal 
with this difficulty, the remote system must have a certain level of intelligence to either 
adapt or avoid these uncertainties. There has been some success with the adapt strategy 
such as force and compliance controlled assembly [25], path planning and replanning [26] 
[27], simulated force/torque reflection control [7] and etc.. But most of them are not quite 
flexible and have only very limited use in real applications. The avoid strategy is the 
most widely used in industrial systems. There are two methods of avoiding uncertainties 
in robot assembly: precision assembly techniques and localization and assembly. The 
precision assembly uses a set of fixtures or jigs to accurately control the position of the 
objects to be operated and thus avoiding the errors during the assembly. The problem 
with this method is its inflexibility and is very difficult to be used in space applications. 
The localization and assembly is therefore a good choice. We are not surprised that the 
“Locate” operation is defined as one of the basic building blocks of E-move in RATS 
[20]. Our last report has already given a detailed discussion about object localization 
techniques. They can be divided into Recognition- Localization Techniques and Direct 
Localization Techniques. Many early object localization algorithms such as [6] [10] [15] 
[23] concentrate on the recognition-localization techniques, technique. The researchers 
have not paid much attention on the direct localization technique until recently they found 
out that a lot of robot applications, especially in assembly and inspection tasks, object 
localization algorithms with high accuracy and real-time execution are the key factors to 
succeed these tasks. For example, in RATS [20], the “Locate” operation should be finished 
in the range of seconds. Those early algorithms are not suitable for this situation. 

Recently, several algorithms which are directed specially at accurate and fast location 
determination have been proposed. One algorithm presented by Gunnarsson and Prinz [8, 
9] uses sparse range data measured on the surfaces of an object to locate the object. Their 
algorithm tries to find a transformation which will make the sum of distances between 
each measured point and its correspondent in transformed surface minimal. Gordon 
and Seering [12] have developed a system which uses striped-light and camera sensing 
system to locate an object. The image projected by a striped-light on the surfaces of an 
object is viewed by a camera and a line-surface matching is then used to carry out the 
transformation parameter estimation. The same striped-light and camera sensing system 
is also used by Rutkowski, Benton and etc. [1, 21]. But their matching strategy is point- 
surface matching. All of the algorithms can be implemented fast and accurately. But if 
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Figure 1: Geometry of a Laser Based Triangulation Sensor. ([14]) 

the object is a polygon, at least three surfaces need to be accessed. Also, the iterative 
numerical procedures used by some of the algorithms have convergence problems. 

In this paper, we present an object localization algorithm which is based on line- 
line matching. A high precision laser line range sensor is used to make a sequence of 
measurements on the object and then to extract line features from these measured data. 
The line features could be boundary edges for planar surfaces or axes for surfaces of 
revolution. The line features are matched to corresponding modeled line features. Closed 
form formulas are used to carry out all the computation. The system described in this 
paper requires a highly constrained environment. That is, either the environment is 
a highly-structured or the position relationships among the objects in the environment 
have previously been established approximately. This is a reasonable assumption for many 
space applications. With this assumption, fairly fast and reliable measurements can be 
taken, which in turn will lead to a fast and accurate localization. 

2 Sensing System 

2.1 Sensor Requirements 

There are a number of three-dimensional range sensing systems [17]. In our application, 
the sensing system should meet the following requirements: 

• A high measurement rate: each complete measurement (including the time spent on 
feature extraction and location computation) should be finished within one second. 

• High measurement accuracy. 

• Capable of extracting line-segment features from sensed object. 
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Figure 2: Line range sensor configuration 

A line range sensor manufactured by CyberOptics Corp. was selected as a prototype 
sensor in our experiments because it satisfies with most of the requirements. This sensor 
is a laser triangulation based range sensor and has the following specifications: [5] 

• Measurement rate: 5 frames /second. 

• Standoff: 30 mm. 

• Depth range: 4 mm. 

• Line length: 2 mm. 

• The number of pixels along the line: up to 64. 

• Lateral resolution: 30 micrometer. 

• Depth resolution: 10 micrometer. 

• Field of View: 30 degree. 

2.2 3-D Range Sensing Fundamentals 

The triangulation based range finding technique uses a known geometric structure between 
the source of laser beam and the detectors to determine the distance. Figure 1 shows the 
geometry of such sensors making a spot measurement. Any position along the laser source 
beam or range can be determined from the position image on the detector by using the 
relationship: 

ry 

Z T = h x tan[tan -1 (-j^) + tan -1 ( -v r)] (see[14]) 

h a COS D 

The line range sensor uses the same principle. If we assign a left-hand coordinate system 
to the sensor (see Fig.2), the coordinates of points of the object in 3-D space lying on the 
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Figure 3: The boundary edge sensing patterns 


projected line may be derived as a function of the readings of the sensor detector array. 
Let# be the angle of filed of view, k the number of pixels of the detector array, and e, the 
range reading of pixel i, where 1 < i < k, then the coordinates of the point v, in space 
corresponding to pixel i is 


Vf = 


^tan 0(^2 - i±£) ei 
0 
ei 


This information will be used in the next section as the input to extract line-segment 
parameters. 


3 Feature Extraction and Location Determination 

The feature extraction and localization process basically consists of two steps: the line- 
segment parameter extraction step and location determination step. Two cases are con- 
sidered in the line-segment feature extraction step: 

1. The line-segment to be extracted is a boundary edge of a planar surface; 

2. The line-segment to be extracted is an axis of a surface of revolution. 

In the following, we will give a brief description of the extraction process for each type of 
line-segment. 


3.1 Boundary Edge Parameter Extraction 

Any boundary edge of a planar surface can be considered as an intersection of two planes.. 
One is the planar surface itself. Another plane is the one perpendicular to it. The process 
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of extracting boundary edge parameters is first to take a sequence of measurements using 
the line range sensor along the boundary edge of the planar surface, and then to determine 
parameters of the two mentioned planes from these measured data. 

After each measurement, the following processing tasks are performed: 

1. Detect boundary point: determine whether there exists any readings in the sensor 
detector array which is corresponding to a boundary point of the object surface 
and locate it if the answer is yes. The detection of boundary point is done by 
analyzing the patterns of readings from the detector array. One possible pattern of 
the readings is that a part of the cells of the detector array has no output while in 
other part of the cells the readings do exist. Another pattern is a change in slope 
in the readings. These patterns correspond to the cases (a) and (6) or (c) of Fig. 3 
respectively. 

2. Eliminate bad readings to reduce the inaccuracies due to noise and object distortion. 
The elimination is done by a histogram technique. 

Having found the boundary point and eliminated the unreliable data from the mea- 
surements, we are ready to compute the needed line-segment parameters. First, all the 
remaining data measured on the planar surface are used to determine the parameters 
of the surface. A least squares fitting can be used. Suppose the equation of the planar 
surface to be determined is ax + by + z = d, we need to find the a, 6 and d from a set of k 
readings (x,-, y,-, z t ), where i = 1, • • • , k and k > 3. The following equation can be obtained 
and used for a least squares optimization and can be solved by a closed form formula: 


' Xi y x 1 ' 


r -i 


' Z\ ■ 

*2 V2 1 
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-*2 
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b 
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Once the plane equation is obtained, the surface normal n = (n x , n y , n z ) can be calcu- 
lated from it. Next, we need to find out the equation for the second plane. This is done by 
a projection process. That is, a rotation matrix M is made in such a way that the z axis 
in the coordinate system is rotated to the direction of the normal vector n of the plane, 
see Fig. 4. All the boundary points will then be projected on the plane by multiplying M 
with these points and taking only x and y components of the multiplication. These pro- 
jected two dimensional boundary points are used to compute an optimal two dimensional 
line-segment. Back projection of the two dimensional line-segment will give the required 
plane equation. The rotation matrix has the property that it is an orthonormal matrix 
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Figure 4: The use of projection method to find the equation of second plane 


and that Mn = (0 0 l) 3 ’. The matrix is not unique. One of them has the form: 


M = 



where g = sjn 2 + n 2 . 

Thus, the line-segment in the original coordinate system can be expressed as the following 
form: 

f aix + biy + z = d x 
\ a 2 x + b 2 y + z = d 2 

Other forms of representation can be obtained from it easily. 

3.2 Axis Parameter Extraction 

The most frequently seen surfaces of revolution in industry include cylindrical surface, 
elliptical surface and conic surface. There are several methods of extracting axis parame- 
ters from range data [4, 11, 16, 22]. Among them, the one from [11] is that most suitable 
for our application. Although their method only describes the ways of extracting the 
parameters of cylindrical surface, more complex surfaces can be analyzed in the same 
way. In this method, only two scans on the cylindrical surface are needed to extract axis 
parameters. See Fig. 5, pick two points on one scan, a,(i — 1,2); the points should be 
widely separated on the scan. The method will try to select two points bi(i = 1, 2) on the 
second scan so that the dot product of the two unit vectors v,-,,-, (t = 1,2) from a, pointing 
to bi is close to 1. Once the points 6, have been found, the unit vectors formed by these 
two pairs of points ((a l5 &i), ( 02 , b 2 )) are the representation of the direction vector of the 
axis. Once the axis is known, the estimate of the axis displacement can be computed 



Figure 5: The extraction of axis from two scans ([11]) 


easily. The projection method used in last section can also be applied here. The differ- 
ence is that instead of projecting a line-segment on the projecting plane, an arc will be 
projected on the plane and the parameters of the arc including the radius and the center 
of the arc will be calculated from the projected data. The two dimensional coordinates of 
the center wii then be back-projected in the original coordinate system and used as the 
displacement of the axis. 

3.3 Location Determination 

Suppose two non-parallel line-segments, boundary edges or axes, have been extracted, 1, 
* = 1,2 , and are expressed as 

I, = lo, + thi i = 1,2 — oo < t < +oo ' (1) 

where lo, are the vectors from the origin of the sensing coordinate system perpendicular 
to and intersecting the lines the line-segments lie on, and n,- are the unit direction vectors 
of the line-segments viewed from the sensing coordinate system. Their corresponding 
line-segments 1, in object model are expressed as 

1* = lo< -b * = 1,2 — oo < t < +oo (2) 

where lo, are the vectors from the origin of the object coordinate system perpendicular to 
and intersecting the lines the line-segments lie on, and n,- are the unit direction vector of 
the line-segments. The location determination is to find a transformation matrix T such 
that 

h = Tl, » = 1,2 

3.3.1 Determining the Rotation 

The rotation can be represented in several ways. If it is represented as a rotation axis p 
of magnitude 1 and a rotation angle a, the rotation can be easily derived by using the 



fact that the rotation axis should be perpendicular to both (ni-fii) and (n 2 -n 2 ). In fact, 
given any two pairs of vectors, the rotation parameters can always be derived. The p has 
the form 

_ (nx - n t ) x (n 2 - n 2 ) 

P ||(ni - nj) x (n 2 -n 2 )|| 

to within an ambiguity of degree of t. Once we have computed a direction vector p, the 
rotation angle can be derived by using the following formula: 


cos a = 


n, • n, - (p • n,) 2 

1 - (p - n,-) 2 


Given values for p and a, the rotation part of the transformation matrix can be deter- 
mined. 


3.3.2 Determining the Translation 

When considering the determination of the displacement, we have to deal with two possible 
cases: e.g., the two line-segments are located in the same plane and they are in different 
planes. 

In the first case, see Fig 6, the intrinsic geometric properties of the line-segments are 
used to derive the formulas. From the given conditions, the intersection point p of the 
two lines emitted from the line-segments, the plane normal vector n„ the distance d of the 
origin from the plane, all expressed in the object coordinate system, can be calculated. 
Also from the equation 

p + ani + &n 2 — dn 3 = o 

and using the properties of vector algebra, we can solve for the parameters a and b: 

a = ((dn 3 - p) - &n 2 ) • ni 

_ (<fn 3 - p) • n 2 - ((dn, - p) • n^nx • n 2 ) 

1 - (nx • n 2 ) 2 
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Figure 7: The cross points of two line-segments 


Using the constants d, a, b and the extracted line-segment parameters, the translation 
vector t is given by 

t = p + arii + &n 2 — dn 4 

where p, n, are the intersection point and plane normal expressed in sensing coordinate 
system, and can be computed from the the extracted line-segment. The correctness of the 
equation (3.3.2) is based on the fact that during any transformation, the relative position 
and orientation of all vectors are unchanged. 

For the second case, e.g., two line-segments are not in the same plane, we can first 
find the two cross points of a line which is perpendicular to both of the two line-segments 
and has the shortest distance. See Fig 7. Once the cross points have been found, all the 
computations used in case one can be applied to derive the displacement vector. 

The coordinates of cross points can be derived by computing the corresponding t values 
of each of the line-segment equations (1) and (2): 


t _ (Ip, - lo») • n t - (n t • n 2 )(l 0l - /o2) • n 2 
1 (ni • n 2 ) 2 - 1 


(3) 


and 

j _ ~(Iqi — fo2) • n 2 + (ni • n 2 )(lo t — lp,) • ni 
2 (ni • n 2 ) 2 - 1 


(4) 


Because the two line-segments are non-parallel, (nj • n 2 ) 2 — 1 will not be equal to zero 
and therefore we do not need to consider the overflow problem here. 


4 Using Multi- Line Features to Locate Objects 

As we have mentioned, two line segments are enough to completely specify transforma- 
tion parameters. Sometimes, multi-line parameters can be extracted. These multiple 
measurements may be used to improve the accuracy of the measurements. In this section, 


we present an efficient algorithm which is based on the use of dual number quaternions 
[24]. The method solves for the orientation and position of an object by minimizing a 
single cost function associated with the sum of the orientation and position errors. In 
the following, quaternions are represented with a boldface script character, such as g; 
vectors are represented with a boldface Roman character, such as q; and dual quantities 
are represented with hat over a character, such as q. 

Quaternions have long been used as a method to represent orientation [13, 19]. The 
extension of quaternions to include the representation of position and orientation is made 
by simply changing all of the quantities in the quaternion to dual quantities. Furthermore, 
the dual quaternion has a similar interpretation as the real quaternion: 


sin(0/2)h 

cos(0/2) 


(5) 


where the vector n is now a dual number quaternion representing the unit vector about 
which the coordinate system has rotated and translated and $ is the dual number quater- 
nion representing the angle of rotation and distance of translation, as illustrated in Fig. 
8. The dual vector h and dual angle 9 are 


n = n + epxn 
9 = 9 + ed 


where n is the axis of rotation and p is the position vector of any point along the line 
of rotation and translation, 9 is the angle of rotation and d is the distance of translation 
along the line. 

The dual quaternion can also be represented in another form: 

q = r + es (6) 


where r and s are both real quaternions and are called the real and dual parts, respectively. 
From equation 5 and 6, we have the following relationships: 


sin(0/2)n 

cos(0/2) 


(7) 


and 


d/2cos(0/2)n + sin(0/2)p x n 
—d/ 2sin(0/2) 


( 8 ) 


Two useful matrix functions 
defined as: 


of quaternions are the matrices Q(r) and W(r) which are 


<?(<•) = 


r,/ + lf(r) r 

r* * 


(9) 
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Figure 8: Illustration of rotation and translation for dual quaternion ([24]) 

m <■)= T>I z* {r) r rt (io) 

where K(r) is the skew-symmetric matrix 

0 — r 3 r 2 

K(r) = r 3 0 -r x (11) 

— r 2 r x 0 

Important properties of these quaternion matrices can be found in [24]. 

In our application, a line segment is characterized by the pair (n,p), where n is a unit 
direction vector of the line segment and p is the distance vector to the origin. We also 
define the position quaternion p and direction quaternion n as the following: 



Now, suppose m line-segments have been extracted and represented in quaternion form 
(Pi ,ni); and (p?,n?) are the corresponding quaternion representation of modeled line- 
segments, where m > 2 and i = 1, • • • , m. If the estimate of transformation is represented 
as q = r + es, the estimated line-segment parameters (p,-,n,) will be 

p , : = W(r) r s + W(t) t Q(t)p\ (12) 
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and 


m = W(r) r Q(rK (13) 

The approach for computing the position and orientation of the object is to determine r 
and s which minimizes the error between the p, and p t and the hi and n t . That is, we 
select the r and s to minimize the following error function: 

m 

E = - n,f + AC*»i - Pi) 3 ) (14) 

1=1 

where a;’s and /3,’s are weighting factors. 

Using the properties of dual number quaternions, we can rewrite the error function 
into the following form 


E = r T C\r + s r Ci8 + s T C 3 r + constant (15) 

where 

Cl = f;(-2a,<?(n,) 3 'W(nf) - 2ftQ(p i ) I W(p?)) (16) 

i=l 

m 

Ci = C£i3,)I (17) 

1=1 
m 

C 3 = 2j> j (W(p?)-Q(p i )) (18) 

t=l 

After considerable matrix algebra, the equation (15) may be written as: 

E = constant — A (19) 

where A is the eigenvalue of the matrix R: 

R = i(C^(C 2 + Cl)- l C 3 -Cr- CT) (20) 

and p is the corresponding eigenvector. Thus, the error is minimized if we select the 
eigenvector r corresponding to the largest positive eigenvalue. 

Having computed r, the s can be obtained by using the equation: 


s — — (C 2 + Cl) l C 3 r 


( 21 ) 


The corresponding 3x3 rotation matrix A can be written in terms of the components of 
the quaternions in the following way: 


A 0 

0 T 1 


W(r) T Q{r) 


( 22 ) 
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and the position vector can be written in the form 

p = W(rfs 


( 23 ) 


In a summary, the method first computes matrix R, then finds the maximum eigen- 
value and its corresponding eigenvector of the matrix, and finally derives the transforma- 
tion parameters from it. Among these steps, the computation of matrix R spends most 
of the time. The complexity of the algorithm is O(m). 

5 Discussion 

We have presented our object localization algorithm based on line-segment matching. The 
method is very simple and computationally fast. In most cases, closed-form formulas are 
used to derive the solution. The method is also quite flexible, because only few surfaces 
(one or two) need to be accessed (sensed) to gather necessary range data. For example, if 
the line-segments are extracted from boundaries of a planar surface, only parameters of 
one surface and two of its boundaries need to be extracted, as compared with traditional 
point-surface matching or line-surface matching algorithms which need to access at least 
three surfaces in order to locate a planar object. Therefore, this method is especially 
suitable for applications when an object is surrounded by many other work pieces and 
most of the object is very difficult, if not impossible, to be measured; or when not all parts 
of the object can be reached. The theoretical ground on how to use line range sensor to 
locate object has been laid on this paper. Much work has to be done in order for the 
method to be really useful. Currently we are doing the following things: 

1. Error analysis including theoretical analysis and simulation result. 

2. Calibration of the sensor. 

3. Experiments to use the sensor to take measurements on objects with different shapes 
and materials. 
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